#include "PID.h"
#include "encoder.h"
#include "tim.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "motor.h"

//传感器
float pitch,roll,yaw;
int Encoder_Left,Encoder_Right;
short gyrox,gyroy,gyroz;
short aacx,aacy,aacz;

//闭环控制中间变量
int Vertical_Out,Velocity_Out,Turn_Out;
int Target_Speed,Target_Turn;
float Move_Angle; //平衡偏移
int motor1,motor2;

//PID
float Vertical_Kp,Vertical_Kd;
float Velocity_Kp,Velocity_Ki;
float Turn_Kp,Turn_Kd;
uint8_t stop;

//直立环PD
//输入 期望∠，真实∠，角速度值
int Vertical(float ExpArg,float CurArg,float gyro_y)
{
	int temp;
	temp=Vertical_Kp*(CurArg-ExpArg)+Vertical_Kd*(gyro_y);
	return temp;
}

//速度环PI
//输入 期望速度，左右编码器
int Velocity(int Target,int Encoder_left,int Encoder_right)
{
	static int Error_Lowout_Last,Error_S;
	static float a=0.7;
	
	int Error,Error_Lowout,temp;
	//偏差值
	Error=(Encoder_left+Encoder_right)-Target;
	//低通滤波
	Error_Lowout=(1-a)*Error+a*Error_Lowout_Last;
	Error_Lowout_Last=Error_Lowout;
	//积分
	Error_S+=Error_Lowout;
	//限幅
	if(Error_S>20000)
	{
		Error_S=20000;
	}
	if(Error_S<-20000)
	{
		Error_S=-20000;
	}
	if(stop==1)
	{
		Error_S=0;
		stop=0;
	}
	//速度环
	temp=Velocity_Kp*Error_Lowout+Velocity_Ki*Error_S;
	return temp;
}

//转向环PD
//输入 角速度值,角度值
int Turn(float gyro_z,int Target_turn)
{
	int temp;
	temp=Turn_Kp*Target_turn+Turn_Kd*gyro_z;
	return temp;
}

void Control()  //10ms定时
{
	int PWM_out;
	//读取编码器和陀螺仪的数据
	Encoder_Left=Read_Speed(&htim2);
//	Encoder_Right=-Read_Speed(&htim4);
	mpu_dmp_get_data(&pitch,&roll,&yaw);
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
	//传数据与计算
	Velocity_Out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);
	Vertical_Out=Vertical(Velocity_Out+Move_Angle,roll,gyrox);
	Turn_Out=Turn(gyroz,Target_Turn);
	PWM_out=Vertical_Out;
	motor1=PWM_out-Turn_Out;
	motor2=PWM_out+Turn_Out;
	Limit(&motor1,&motor2);
	Load(motor1,motor2);
}